import time
import cv2
from driver_server import VSRController
# from driver_server import IOACamera

import modbus_tk.defines as cst
import modbus_tk.modbus_tcp as modbus_tcp
import socket

### 四轴机械臂点位
## 1开头--2待取料--3取料--4抬高--5待放--6放料--7货叉出仓--8抬高
P1=[0.0,10.45,62.05,0.0,0.0,0.0,146.011,0.0,143.008,0.0,0.0,0.0,0.0]
P2=[81.1, 65.3, 86.3, 8.1, 0.0, 0.0, 29.5, 189.0, 47.7, 8.1, 0.0, 0.0, 0.0]
P3=[82.9, 69.9, 67.4, 8.1, 0.0, 0.0, 29.5, 238.5, 47.7, 8.1, 0.0, 0.0, 0.0]
P4=[82.9, 63.8, 64.7, 8.1, 0.0, 0.0, 29.5, 238.5, 64.2, 8.1, 0.0, 0.0, 0.0]
P5=[0.0,0.0,0.0,0.0,0.0,0.0,198.67,0.0,230.72,0.0,0.0,0.0,0.0]

P6=[-24.1, 71.6, 80.1, 112.1, 0.0, 0.0, 193.1, -87.4, 35.3, 112.1, 0.0, 0.0, 0.0]
P7=[-47.9, 74.4, 69.8, 125, 0.0, 0.0, 159.8, -176.7, 35.3, 125, 0.0, 0.0, 0.0]
P8=[-50.6, 52.7, 32.7, 132.2, 0.0, 0.0, 180.5, -219.6, 138.3, 132.2, 0.0, 0.0, 0.0]

### 六轴机械臂点位
## 1号仓位待取--取料--待放料--放料
P9=[-37.5, 15.9, -5.1, 3.4, -11.8, -1.0, 182.9, -138.5, 194.6, 3.4, -0.9, -39.2, 0.0]
P10=[-38, 22.1, 1.7, 4.8, -25.7, -1.6, 185.7,-142.5, 156.3, 2.7, 3.2, -39.2, 0.0]
P11=[-6.0, 38.6, 0.3, 0.0, -39.8, 17.7, 240.3, 25.2, 96.6, -0.3, -0.9, 11.7, 0.0 ]
P12=[-6.0, 30.5, -0.5, 0.2, -32.7, 21, 240.3, 128.2, 96.6, -0.3, -0.9, 11.7, 0.0 ]

## 过渡点
P13=[-29.175,0.0,0.0,0.0,0.0,0.0,173.466,-96.846,230.72,0.0,0.0,-29.175,0.0]

## 2号仓位 待取--取料--待放料--放料
P14=[-49.8, 29.8, -16.3, 2, -4.7, 2.1, 161.2, -189.3, 176.7, 2.3, 8.8, -47.8, 0.0]
P15=[-49.8, 31.9, -11.7, 2.1, -11.3, 2.4, 161.2, -189.3, 155.1, 2.3, 8.8, -47.8, 0.0]
P16=[-2.6, 56.1, -31.4, 0.0, -24.8, 24.5, 280.9 -12.9, 110.2, 0, 0, 21.9, 0.0]
P17=[-2.6, 60, -31.4, 0.0, -28.6, 24.5, 280.9 -12.9, 93.2, 0, 0, 21.9, 0.0]

## 3号仓位 待取--取料--待放料--放料
P18=[-60.3, 26.6, -12.0, 0.4, -5.4, 12.4, 120.4, -210.8, 176.7, 2.3, 8.8, -47.8, 0.0]
P19=[-61.1, 27, -4.9, 0.3, -13, 13.2, 115.6, -209, 154.3, 2.3, 8.8, -47.8, 0.0]
P20=[-10.7, -75, 63.0, 5.3, -16.3, 19.6, 300.4, -54.7, 115.5, 3.5, -5.8, 7.4, 0.0]
P21=[-9.5, -80.5, 63.4, 5.3, -16.9, 20.7, 299.8, -47.9, 90.3, 4.8, -1.6, 9.6, 0.0]

## 4号仓位 待取--取料--待放料--放料
P22=[-72.7, 19.3, 0.6, -1.6, -10.9, 24.4, 66.8, -217.2, 166.3, 2.3, 8.8, -47.8, 0.0]
P23=[-72.7, 21.3, 3.1, -1.6, -15.5, 24.2, 66.8, -217.2, 151.9, 2.3, 8.8, -47.8, 0.0]
P24=[-14.3, 47.8, -19.7, 0.0, -28.1, 36.2, 259.9, -66.3, 113.4, 0.0, 0.0, 15.6, 21.9]
P25=[-14.4, 52.3, -19.7, 0.0, -32.7, 36.2, 259.9, -66.3, 94.4, 0.0, 0.0, 15.6, 21.9]

### 回仓1号待取点--取料--待放料--放料
P26=[27.8, 33.3, 3.8, 0.0, -37.1, -14.0, 208.7, 109.9, 107.3, 0.0, 0.0, 13.8, 0.0]
P27=[27.8, 38.1, 3.6, 0.0, -41.7, -14.0, 208.7, 109.9, 90.3, 0.0, 0.0, 13.8, 0.0]
P28=[-36.3, -1.4, 28.6, 0.0, -27.2, 4.1, 150.3, -110.4, 151.1, 0.0, 0.0, -32.2, 0.0]
P29=[-36.3, 5.3, 31.8, 0.0, -37.1, 4.1, 150.3, -110.4, 124.3, 0.0, 0.0, -32.2, 0.0]

### 回仓2号待取点--取料--待放料--放料
P30=[27.7, 60.4, -40, 0.0, -20.4, -13.9, 255.9, 134.3, 115.9, 0.0, 0.0, 13.8, 0.0]
P31=[27.7, 66.1, -40, 0.0, -26, -13.9, 255.9, 134.3, 90.3, 0.0, 0.0, 13.8, 0.0]
P32=[-51.5, 7.6, 28.2, 0.0, -35.8, -8.3, 119.9, -150.8, 127.7, 0.0, 0.0, -59.8, 0.0]
P33=[-51.5, 13.9, 29.2, 0.0, -43.1, -8.3, 119.9, -150.8, 106.7, 0.0, 0.0, -59.8, 0.0]

### 回仓3号待取点--取料--待放料--放料
P34=[18.9, 48.9, -24.8, 0.0, -24.1, -5.2, 258.7, 88.7, 123, 0.0, 0.0, 13.8, 0.0]
P35=[18.9, 55.6, -24.2, 0.0, -31.4, -5.2, 258.7, 88.7, 92.7, 0.0, 0.0, 13.8, 0.0]
P36=[-64.6, 4.9, 33.2, 0.0, -38.1, -22.8, 79.1, -166.4, 121.9, 0.0, 0.0, -87.4, 0.0]
P37=[-64.6, 12.1, 33.8, 0.0, -46.6, -22.8, 79.1, -166.4, 100.7, 0.0, 0.0, -87.4, 0.0]

### 回仓4号待取点--取料--待放料--放料
P38=[18.8, 35.4, -0.8, 0.0, -34.8, -5.1, 229.5, 78.3, 110.7, 0.0, 0.0, 13.8, 0.0]
P39=[18.8, 41.3, -0.8, 0.0, -40.4, -5.1, 229.5, 78.3, 89.5, 0.0, 0.0, 13.8, 0.0]
P40=[-78.1, -6.8, 43.3, 0.0, -36.5, -35.2, 33.9, -161.2, 125.5, 0.0, 0.0, -113.4, 0.0]
P41=[-78.1, 2.9, 44.3, 0.0, -47.1, -35.2, 33.9, -161.2, 100.3, 0.0, 0.0, -113.4, 0.0]

#### x,y 检测时皮带机上托盘仓位的像素坐标,左上角（x_1,y_1）右下角（x_2,y_2）
x_1 = 242
x_2 = 378
y_1 = 178
y_2 = 298

def run_demo():
    try:
        controller.connect()
        ## 示例机械臂运动指令
        # controller.set_joint_angle(com_Pose= P1, com_Num=1)

        ## 示例：控制输出接口
        # controller.SetDO(0, 1)
        # time.sleep(2)
        # controller.SetDO(0, 0)

        ## 六轴机械臂：取料到货架--机械臂动作流程
        getGoods()

        ## 四轴机械臂：货叉--插取物料托盘到传送带
        putGoodsPlate()

        ### 颜色识别
        # process_master.master_.execute(slave=1, function_code=cst.WRITE_SINGLE_REGISTER, starting_address= 181, output_value= 1)
        ### 形状识别
        # process_master.master_.execute(slave=1, function_code=cst.WRITE_SINGLE_REGISTER, starting_address= 181, output_value= 2)
        ### QR 识别
        process_master.master_.execute(slave=1, function_code=cst.WRITE_SINGLE_REGISTER, starting_address= 181, output_value= 3)
        ### OCR
        # process_master.master_.execute(slave=1, function_code=cst.WRITE_SINGLE_REGISTER, starting_address= 181, output_value= 4)


        ## 示例：视觉相机获取（IOA）
        # res, points = qrcode_detect()
        # print(f"qr det result is {res}")
        
        # if points is not None:
        #     for idx, qr_points in enumerate(points):
        #         x0, y0 = qr_points[0]  
        #         x1, y1 = qr_points[2]  # 假设 qr_points 是按顺时针或逆时针顺序存储的四个角点  
        #         center_x = (x0 + x1) / 2  
        #         center_y = (y0 + y1) / 2  
        #         QR_value = int(res[idx])
        #         if abs(x_1 - center_x) < 30 and  abs( y_1 - center_y) < 30:
        #             for i in range(1,5):
        #                 if i == QR_value:
        #                     exe_action1()
        #                     matchQR2warehouse(i)
                
        #         elif abs(x_1 - center_x) < 30 and  abs( y_2 - center_y) < 30:
        #             for i in range(1,5):
        #                 if i == QR_value:
        #                     exe_action2()
        #                     matchQR2warehouse(i)
                
        #         elif abs(x_2 - center_x) < 30 and  abs( y_1 - center_y) < 30:
        #             for i in range(1,5):
        #                 if i == QR_value:
        #                     exe_action3()
        #                     matchQR2warehouse(i)
                
        #         elif abs(x_2 - center_x) < 30 and  abs( y_2 - center_y) < 30:
        #             for i in range(1,5):
        #                 if i == QR_value:
        #                     exe_action4()
        #                     matchQR2warehouse(i)


        #         # if "1" in res:
        #         #     exe_action1()
                
        #         # elif "2" in res:
        #         #     exe_action2()
                
        #         # elif "3" in res:
        #         #     exe_action3()

        #         # elif "4" in res:
        #         #     exe_action4()

    except Exception as e:
        print(e)
    
    finally:
        if controller._running:
            controller.stop()

##六轴机械臂：取料到货架--机械臂动作流程
def getGoods():
    ### 取1号仓位物料--放托盘
    controller.set_joint_angle(com_Pose= P9, com_Num=1)
    controller.set_joint_angle(com_Pose= P10, com_Num=1)
    controller.SetDO(0,1)
    time.sleep(1)
    controller.set_joint_angle(com_Pose= P9, com_Num=1)
    # controller.set_joint_angle(com_Pose= P13, com_Num=1)
    controller.set_joint_angle(com_Pose= P11, com_Num=1)
    controller.set_joint_angle(com_Pose= P12, com_Num=1)
    controller.SetDO(0,0)
    time.sleep(1)
    controller.set_joint_angle(com_Pose= P11, com_Num=1)
    controller.set_joint_angle(com_Pose= P13, com_Num=1)


    ### 取2号仓位物料--放托盘
    controller.set_joint_angle(com_Pose= P14, com_Num=1)
    controller.set_joint_angle(com_Pose= P15, com_Num=1)
    controller.SetDO(0,1)
    time.sleep(1)
    controller.set_joint_angle(com_Pose= P14, com_Num=1)
    # controller.set_joint_angle(com_Pose= P13, com_Num=1)
    controller.set_joint_angle(com_Pose= P16, com_Num=1)
    controller.set_joint_angle(com_Pose= P17, com_Num=1)
    controller.SetDO(0,0)
    time.sleep(1)
    controller.set_joint_angle(com_Pose= P16, com_Num=1)
    controller.set_joint_angle(com_Pose= P13, com_Num=1)

    ### 取3号仓位物料--放托盘
    controller.set_joint_angle(com_Pose= P18, com_Num=1)
    controller.set_joint_angle(com_Pose= P19, com_Num=1)
    controller.SetDO(0,1)
    time.sleep(1)
    controller.set_joint_angle(com_Pose= P18, com_Num=1)
    # controller.set_joint_angle(com_Pose= P13, com_Num=1)
    controller.set_joint_angle(com_Pose= P20, com_Num=1)
    controller.set_joint_angle(com_Pose= P21, com_Num=1)
    controller.SetDO(0,0)
    time.sleep(1)
    controller.set_joint_angle(com_Pose= P20, com_Num=1)
    controller.set_joint_angle(com_Pose= P13, com_Num=1)

    ### 取4号仓位物料--放托盘
    controller.set_joint_angle(com_Pose= P22, com_Num=1)
    controller.set_joint_angle(com_Pose= P23, com_Num=1)
    controller.SetDO(0,1)
    time.sleep(1)
    controller.set_joint_angle(com_Pose= P22, com_Num=1)
    # controller.set_joint_angle(com_Pose= P13, com_Num=1)
    controller.set_joint_angle(com_Pose= P24, com_Num=1)
    controller.set_joint_angle(com_Pose= P25, com_Num=1)
    controller.SetDO(0,0)
    time.sleep(1)
    controller.set_joint_angle(com_Pose= P24, com_Num=1)
    controller.set_joint_angle(com_Pose= P13, com_Num=1)

    time.sleep(1)

## 四轴机械臂：货叉--插取物料托盘到传送带
def putGoodsPlate():
    time.sleep(1)
    controller.set_joint_angle(com_Pose= P1, com_Num=2)
    controller.set_joint_angle(com_Pose= P2, com_Num=2)
    controller.set_joint_angle(com_Pose= P3, com_Num=2)
    controller.set_joint_angle(com_Pose= P4, com_Num=2)
    controller.set_joint_angle(com_Pose= P5, com_Num=2)
    controller.set_joint_angle(com_Pose= P6, com_Num=2)
    controller.set_joint_angle(com_Pose= P7, com_Num=2)
    controller.set_joint_angle(com_Pose= P8, com_Num=2)
    time.sleep(1)
    controller.SetDO(1,1)
    time.sleep(6)
    controller.SetDO(1,0)
    time.sleep(1)


## 二维码检测识别
def qrcode_detect():
    img = camera.get_image()

    detect_obj = cv2.wechat_qrcode_WeChatQRCode('detect.prototxt', 'detect.caffemodel', 'sr.prototxt', 'sr.caffemodel')
    res, points = detect_obj.detectAndDecode(img)

    # ## 获取二维码图像
    # qr = cv2.QRCodeDetector()
    # ## 解析二维码信息
    # result, points, code = qr.detectAndDecode(img)
    # 输出解析信息
    return res, points

## 回仓匹配
def matchQR2warehouse(i):
    if i == 1:
        controller.set_joint_angle(com_Pose= P28, com_Num=1)
        controller.set_joint_angle(com_Pose= P29, com_Num=1)
        controller.SetDO(0, 0)
        time.sleep(0.5)
        controller.set_joint_angle(com_Pose= P28, com_Num=1)
    elif i == 2:
        controller.set_joint_angle(com_Pose= P32, com_Num=1)
        controller.set_joint_angle(com_Pose= P33, com_Num=1)
        controller.SetDO(0, 0)
        time.sleep(0.5)
        controller.set_joint_angle(com_Pose= P32, com_Num=1)
    elif i == 3:
        controller.set_joint_angle(com_Pose= P36, com_Num=1)
        controller.set_joint_angle(com_Pose= P37, com_Num=1)
        controller.SetDO(0, 0)
        time.sleep(0.5)
        controller.set_joint_angle(com_Pose= P36, com_Num=1)

    elif i == 4:
        controller.set_joint_angle(com_Pose= P40, com_Num=1)
        controller.set_joint_angle(com_Pose= P41, com_Num=1)
        controller.SetDO(0, 0)
        time.sleep(0.5)
        controller.set_joint_angle(com_Pose= P40, com_Num=1)


## 控制机械臂抓取放料
def exe_action1():
    controller.set_joint_angle(com_Pose= P26, com_Num=1)
    controller.set_joint_angle(com_Pose= P27, com_Num=1)
    controller.SetDO(0, 1)
    controller.set_joint_angle(com_Pose= P26, com_Num=1)
    controller.home(com_Num=1)

    # controller.set_joint_angle(com_Pose= P28, com_Num=1)
    # controller.set_joint_angle(com_Pose= P29, com_Num=1)
    # controller.SetDO(0, 0)
    # time.sleep(0.5)
    # controller.set_joint_angle(com_Pose= P28, com_Num=1)

def exe_action2():
    controller.set_joint_angle(com_Pose= P30, com_Num=1)
    controller.set_joint_angle(com_Pose= P31, com_Num=1)
    controller.SetDO(0, 1)
    controller.set_joint_angle(com_Pose= P30, com_Num=1)
    controller.home(com_Num=1)

    # controller.set_joint_angle(com_Pose= P32, com_Num=1)
    # controller.set_joint_angle(com_Pose= P33, com_Num=1)
    # controller.SetDO(0, 0)
    # time.sleep(0.5)
    # controller.set_joint_angle(com_Pose= P32, com_Num=1)

def exe_action3():
    controller.set_joint_angle(com_Pose= P34, com_Num=1)
    controller.set_joint_angle(com_Pose= P35, com_Num=1)
    controller.SetDO(0, 1)
    controller.set_joint_angle(com_Pose= P34, com_Num=1)
    controller.home(com_Num=1)

    # controller.set_joint_angle(com_Pose= P36, com_Num=1)
    # controller.set_joint_angle(com_Pose= P37, com_Num=1)
    # controller.SetDO(0, 0)
    # time.sleep(0.5)
    # controller.set_joint_angle(com_Pose= P36, com_Num=1)

def exe_action4():
    controller.set_joint_angle(com_Pose= P38, com_Num=1)
    controller.set_joint_angle(com_Pose= P39, com_Num=1)
    controller.SetDO(0, 1)
    controller.set_joint_angle(com_Pose= P38, com_Num=1)
    controller.home(com_Num=1)

    # controller.set_joint_angle(com_Pose= P40, com_Num=1)
    # controller.set_joint_angle(com_Pose= P41, com_Num=1)
    # controller.SetDO(0, 0)
    # time.sleep(0.5)
    # controller.set_joint_angle(com_Pose= P40, com_Num=1)


class process_Master():
    def __init__(self):
        super().__init__()
        self.host = socket.gethostname()
        self._master_init()

    def _master_init(self):
        self.master_ = modbus_tcp.TcpMaster(host=self.host)

if __name__ == "__main__":
    controller = VSRController(simulate=True)
    process_master = process_Master()
    # camera = IOACamera()
    run_demo()

